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KINEMATIC CAPABILITY IN SVDS 


1 . INTRODUCTION 

The purpose of this document is to present the details of the Remote Manipu- 
lator System (RMS) kinematic capability implemented into the Space Vehicle 
Dynamics Simulator (SVDS). Section 2 presents a brief overview of the RMS 
kinematic models as they are interfaced in SVDS. The basic interface shown 
can also be used for the RMS rigid body models and the flexible body models. 
Section 3 contains detailed engineering flow diagrams of the RMS KINMAT model's. 
Section 4 contains the definition of the SVDS RMS KINMAT common variables and 
the user inputs required to execute these options. 

Four appendixes have. been included. Appendix A contains a description of the 
coordinate systems used and some notation and basic mathematical relationships. 
Appendix B presents a development of the equations used to compute joint angles 
given the position and orientation of the end effector. Appehdix C contains a 
development of the Resolved Rate Law, the relationship between the end effector 
translational and rotational velocity and the arm joint rates. It is noted 
that these relationships are transformed to the wrist pitch system so that 
joint rates can be solved explicitly. This is similar to a development pre- 
sented in ref. 1. Appendix D presents a development of the equations used in 
the 1 ine-of-sight steering, and axis-of -rotation steering use by the RMS 
kinematic model. 
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2. SVDS-RMS KINMAT INTERFACE 


The RMS kinematic model implemented into the SVDS program does not interface 
with the dynamics .of the rigid body motion simulated by the SVDS program; 
however, the commanded arm joints are integrated to determine the current 
values -of the arm angles. Because of the requirement to numerically integrate 
these variables, it was decided that this integration should be merged with 
the integration of the rigid body equation of motion. 

For this reason, the RMS model was interfaced at three places in the SVDS 
program: (1) In the integration initialization to set up the additional 

variables to-be integrated, (2) In the integration driver, and (3) In the 
math model driver to compute'.RMS steering commands and compute RMS joint posi- 
tions for interface with future models requiring these computations. 

The method of interface that was chosen can easily be followed for future 
development items, such as RMS rigid body and flexible body dynamics. The 
flowchart following is an overview of the SVDS program structure with- the 
KINMAT interfaces indicated. 
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KINSTR 

(COMPUTE RMS 
STEERING COMMANDS) 


Figure 1.- Overview of SVDS with KINMAT interfaces, 








3. ENGINEERING FLOWCHARTS 


This section contains flowcharts of the subroutines introduced into SVDS to 
provide the RMS kinematic capability. These flowcharts use engineering sym- 
bols and form the basis for the routines coded into SVDS. The purpose of 
this section is to provide logic flow in 'a terminology more familiar to an 
engineer than FORTRAN code. Definitions of symbols used are included at the 
end of this section. 
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SUBROUTINE RMANGS 


Subroutine RMANGS computes the RMS joint angle, given the position and 
orientation. of the end effector relative to the orbiter.* 



* 

See Appendix A for a development of the equations used in this routine. 
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COMPUTE SHOULDER YAW ANGLE 

e 1 = tan'^Cr^dg^rfap) 


COMPUTE TRANSFORMATION MATRIX 
FROM 7 SYSTEM TO 2 SYSTEM 

M -[ T 3( # l)]M 


CHECK FOR SINGULARITY WRIST YAW 
ANGLE EQUAL ±90° 

CK = {1 2 }T [ B 27] {1 1 } : 


ABS(CK) :1 


EQ ' SINGULARITY 

' CONDITION 



COMPUTE WRIST YAW AND ROLL ANGLES 


RETURN 


v=' 5f "> 2 } T y‘v) 

_-l/" {1 2 }T [ B 27] f1 3 1 ' 


e = tan ‘ — - y - p ' — — 

0 . Vv b 2 7 <v; 

“• i 
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COMPUTE ELBOW, SHOULDER, AND WRIST PITCH ANGLES 


N { 1 i> 


$ - tan' 


a = {1-|}^{r}cos 0^ + 0^ 

Y = -n 3 } T (r> - sin 6 ■ 


J^cos B 


( 2 ' 2 
yf + « - 


*2 


^3) • 



ARM CAN'T REACH 
POSITION GIVEN 
(ERROR) 


f 


[ RETURN 

V. . - 



3-4 






SUBROUTINE LTRAJ 


Subroutine LTRAU computes a trapezodial time history for the magnitude of the 
translational velocity, and a unit vector in the direction of the line of • 
sight. 
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SUBROUTINE ATRAJ 


Subroutine ATRAJ computes a trapezodial time history for the magnitude of the 
rotational velocity of the end effector, and a unit vector along the single 
axis of rotation. 
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SUBROUTINE LNKAGE 


Subroutine LNKAGE computes RMS joint positions relative to the orbiter, and 
the orientation (transformation matrices) of these coordinate systems relative 
to the orbiter. 
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' SUBROUTINE KINSTR 


Subroutine KINSTR computes the end effector commanded velocity, and angular 
velocity relative to the orbiter in the of biter body system. 
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COMPUTE ROTATIONAL 
VELOCITY COMMAND 
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SUBROUTINE KINDER 



Subroutine KINDER computes the joint angle rates compatible with the commanded 
translational and rotation velocity of the end effector using the resolved 
rate law. 


START 


TRANSFORM END EFFECTOR VELOCITIES 
TO WRIST PITCH SYSTEM 

{ b-j } = [T 2 (0 2 + 0 3 + 8 4 )][T 3 (0 1 )1 [T lb J{w c } 

{b 2 } = ET 2 (0 2 + °3 + V HT 3 ( 0 1 )J [T lb HV c } 


CALCULATE ELEMENTS OF JACOBIAN 'MATRIX-' 
a-H = - sin (6 2 + 0 3 + 0 4 ) 

a 16 = cos e 5 ' ‘ 
a 26 = Sin 0 5 

a 31 = cos (fl 2 + 9. 3 + 8 4 ) - - ‘ 

a 41 = -' SL 5 cos ^ e 2 + ®3‘ + e 4^ S1n 0 5 

a 51 = C0S 0 2 + *"3 co ^ ^ 0 2 + e 3 ) + ( A 4 + 65 cos 65) (cos (© 2 + 63 + 0^)) 
a 61 = -a 5 sin (Q 2 + 0 3 + e 4 ) sin . 

a 42 = Z 2 sin (83 + e 4 ) + l 3 sin,0 4 

a 62 = C0S ^°3 + e 4^ ” ^3 co ? °4 ~ a 5* cos 6 5 " ^4 

a 43 = £ 3 sin °4- . . ■ 

a 63 = ’ h ■" H C0S 0 5 " *3 C0S °4 

a 64 ” ~ ^4 + £ 5 cos 

a 45 " \*5 S1 ' n °5 • 

= iJ cos 0^ 



SOLVE FOR JOINT RATES 







DEFINITIONS OF SYMBOLS 


. Maximum translational acceleration of the end effector relative 

to the orbiter 

a . . Coefficients of the Jacobian used in the resolved rate law 

* J 

[B] .Transformation matrix from initial orientation of the end effector 

to the final 

[Bp] Transformation matrix from final desired end effector orientation ' 

to orbiter body system 

{b-j} Column matrix of the commanded end effector angular velocity 

relative to the orbiter expressed in - the wrist pitch system 

{b^ } Column matrix of the components of the commanded end effector 

translational velocity relative to the orbiter expressed in the 
wrist pitch system 

[B-jgl Transformation matrix from the upper arm system to the longeron 

system 

[B-j 4 3 Transformation matrix from the lower arm system to .the longeron 

system 

[B.| 5 ] Transformation matrix from the wrist pitch system to the longeron 

system 

[B-jg] Transformation matrix from the wrist yaw system to the longeron 

system 

[B^y] Transformation matrix from the end effector system to the longeron 

system- 

[ B ^ 7 3 Transformation matrix from the end effector system to the shoulder 

yaw system 

D Distance from initial position of end effector to final desired 

position of the end effector 

D m Twice distance traveled by end effector during time to reach 

maximum velocity accelerating at maximum acceleration 
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D. 


m 


r I 






Difference between D and 

m 

Identity matrix 

Distance from RMS orbiter attach point to shoulder pitch pivot 
point 

Length of lower arm 


l. 




a 


. 5 
{R} 


(r) 

{Rp} 


{r lB } 


{r 2B } 


{r 3B* 


{r 4B } 


Length of upper arm 

Distance from wrist pitch joint to wrist yaw joint 
Distance from -wrist yaw joint to tip of end effector 


Column matrix of the component of position vector from initial 
position to final desired position expressed in the orbiter body 
system 


Column matrix of the components of position vector from shoulder 
pitch joint to wrist yaw joint expressed in the longeron system 

Column matrix -of the components of the position vector from the 
center of mass (CM) of the orbiter to the final desired position 
of the tip of the end effector expressed in the orbiter body 
system 

Column matrix of the components of the position vector from the 
CM of the orbiter to the RMS attach point in the orbiter body 
system 


Column matrix of the components of the position vector from the 
CM of the orbiter to the shoulder pitch joint in the orbiter 
body system 


Column matrix of the components of the position vector from the 
CM of the orbiter to the elbow pitch joint in the orbiter body 
system 


Column matrix of the components of the position vector from the 
CM of the orbiter to the wrist pitch joint in the orbiter body 
system 
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{r^} Column matrix of the components of, the, position vector from the CM 

of the orbiter to the yaw wrist joint in the orbiter body system 

{rgg} Column matrix of the components of the position vector from the CM 

of the orbiter to the tip of the end effector in the orbiter body 
system 

} Column matrix of the components of the position vector from the RMS 

orbiter attach point to the shoulder pitch joint in the longeron 
system 

{r^-j > Column matrix of the components of the position vector from the RMS 

orbiter attach point to the elbow pitch joint in the longeron 
system 

f > . Column matrix of the components of the position vector from the RMS 
orbiter attach point to the wrist pitch joint in the longeron 
system 

{r^} Column matrix of the components of the position vector from the RMS 

orbiter attach point to the wrist yaw joint in the longeron system 

{rg-j} Column matrix of the components of the position vector from the RMS 

orbiter attach point to the tip of the end effector in the longeron 
system 

[Tig] Transformation matrix from the orbiter body system to the longeron 

system 

[Igsl Transformation matrix from the lower arm system to the orbiter 

body system 

[Tg^] Transformation matrix from the upper arm system to the orbiter 

body system 

[Tgg] Transformation matrix from the wrist pitch system to the orbiter 

body system 

[Tgg] Transformation matrix from the wrist yaw system to the orbiter 

body system 

[T B7 ] Transformation matrix from the end effector system to the orbiter 

body system 
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tRl Time when max angular velocity is attained 

Time of start of angular velocity deceleration phase 

.Time of end of end effector rotational maneuver 
tyi Time when max velocity of end effector is attained 

t-p2 Time. of start of end effector deceleration phase 

• t-j-3 Time of end of end effector translational maneuver 

IT-j (0)1 Transformation matrix representing a 9 rotation about the l r axis 

IT 2 <e)] Transformation matrix representing a 0 rotation about the 2-axis 

IT 3 (e)] Transformation matrix representing a 0 rotation about the 3-axis 

(u R > Column matrix representing the unit axis of rotation vector 

{Uj} Column matrix representing the unit vector along the line of sight 

V - Magnitude of the translational velocity command 

{V> Column matrix representing a vector in the direction of the 

single -axis of rotation vector 

Column matrix of the- components of the commanded velocity vector 
expressed in the orbiter body system 

v m ' Magnitude of the maximum allowable translational velocity of the 

end effector relative to the orbiter 

* 

Y m Magnitude of the maximum translational velocity attained for the 

end effector relative to the orbiter 

a Parameter used in arm angle initialization computations 

• Magnitude of the maximum allowable angular acceleration of the 
end effector relative to the orbiter 

0 parameter used in arm angle initialization computations 

Y Parameter used in arm angle initialization computations 
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Shoulder yaw angle 
Shoulder pitch angle 
Elbow pitch angle 
Wrist pitch angle 
Wrist yaw angle 
Hand roll angle 
Shoulder yaw angle rate 
Shoulder pitch angle rate 
Elbow pitch angle rate 
Wrist pitch angle rate 
Wrist yaw angle rate 
Hand roll angle rate 

Rotation angle from initial orientation of end effector to final 
desired orientation 

Twice the angular distance traveled in acceleration from zero 
angular rate to max angular rate at max angular acceleration 

Magnitude of commanded angular velocity of end effector relative 
to orbiter 

Column matrix of the components of the commanded angular velocity 
vector for the end effector relative to the orbiter 

Magnitude of ■ the ‘maxi mum allowable angular velocity of the end 
effector 

Magnitude of the maximum angular velocity attained for. the end 
effector 

Column matrix with ith component equal 1 and all other components 


zero 
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4. SVDS-RMS KINMAT COMMON VARIABLES AND DEFINITIONS 


This section 
definitions. 

Variable 
I RMS 

IRMSTR 

RMANGD 

RMSAMR 

RMSAMT 

RMSANG 

RMSB13 


contains a list of the SVDS-RMS KINMAT common variables and their 
Input variables are listed at the end of this section. 

Definition 

RMS angle initialization flag 

=0 Compute RMS angles from position and orientation input data 

RMS steering option flag 
=0 Table lookup of joint rate commands used 
=1 Compute 1 ine-of-sight and axis-of-rotation steering 
=2 Line-of-sight and axis-of-rotation steering has been 
initialized 

(1) Shoulder yaw angle rate 

(2) Shoulder pitch angle rate 

(3) Elbow pitch angle rate 

(4) Wrist pitch angle rate 

(5) Wrist yaw angle rate 

(6) Hand roll angle rate 

Magnitude of maximum allowable angular acceleration of end 
effector relative to orbiter 

Magnitude of the maximum allowable end effector translational 
acceleration relative to the orbiter 

RMS joint angles 

(1) Shoulder yaw angle 

(2) Shoulder pitch angle 

(3) Elbow pitch angle 

(4) Wrist pitch angle 

(5) Wrist yaw angle 

(6) Hand roll angle 

Transformation matrix from the upper arm system to the 
longeron system 
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Variable 

RMSB14 

RMSB15 

RMSB1 6 

RMSB17 

RMSLTH 


RMSRC 

RMSRF 

RMSR1B 

RMSR2B 

RMSR3B 

RMSR4B 

RMSR5B 


Definition 

Transformation matrix from lower arm system to the longeron 
system 

Transformation matrix from the wrist pitch system to the longeron 

■ * 

system 

Transformation matrix from the wrist yaw system to the longeron 
system 

Transformation matrix from the arm end effector system to the 
longeron system 

Arm member lengths 

(1) Length from RMS orbiter attach point to shoulder pitch 
joint 

(2) Length- from shoulder pitch joint to elbow pitch joint 
(3} Length from elbow pitch joint to wrist pitch joint 

(4) Length from wrist pitch joint to wrist yaw joint 

(5) Length from wrist yaw joint to tip of end effector 

Dependent variable in commanded arm angle rates table lookups 

Position vector from the orbiter CM to the final desired position 
of the tip of the end effector in the orbiter body system 

Position vector from the orbiter CM to the RMS attach point in 
the orbiter body system 

Position vector from the orbiter CM to the shoulder pitch joint 
in the orbiter body system 

Position vector from the orbiter CM to the elbow pitch joint in 
the orbiter body system 

Position vector from the orbiter CM to the wrist pitch joint in 
the orbiter body system 

Position vector from the orbiter CM to the wrist yaw joint in the 
orbiter body system 
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Variable 


Definition 


RM5R6B 

RMSR21 

RMSR31 

RMSR41 

RMSR51 

RMSR61 

RMSTBF 

RMSTB3 

RMSTB4 

RMSTB5 

RMSTB6 

RMSTB7 

RMSTLB 

RMSTR1 


•Position vector from the orbiter CM to the tip of the end effector 
in the orbiter body system 

Position vector from the RMS orbiter attach point to the shoulder 
pitch joint in the longeron system 

Position vector from the RMS orbiter attach point to the elbow 
pitch joint in the longeron system 

Position vector from the RMS orbiter attach' point to the wrist 
pitch joint in the longeron system 

Position vector from the RMS orbiter attach point to the wrist 
yaw joint in the longeron system 

Position vector from the RMS orbiter attach point to the tip of 
the end effector in the longeron system 

Transformation matrix from the final desired orientation of the 
end effector to the orbiter body system 

Transformation matrix from the upper arm system to. the orbiter 
body system 

Transformation matrix from the lower arm system to the orbiter 
body system 

Transformation matrix from the wrist pitch system to the orbiter 
body system' ’ • 

Transformation matrix from the wrist yaw system to the orbiter 
body system 

Transformation matrix from the arm end effector system to the 
orbiter body system 

Transformation matrix from the orbiter body system to the longeron 
system • - ■ 

Time relative to RMSTS of the end of the angular acceleration 
phase of axis-of-rotation steering 
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Variable 


Definition 


RMSTR2 

RMSTR3 

RMSTS 

RMSTT1 

RMSTT2 

RMSTT3 

RMSUR 

RMSUT 

RMS VC 

RMSVMR 

RMSVMT 

RMSWC 

RMVMRS 

RMVMTS 


Time relative to RMSTS of the end of the angular rate coast 
phase of axis-of-rotation steering 

Time relative to RMSTS of the end of the axis-of-rotation 
steering maneuver 

Time of the start of the line-of-sight and axis-of-rotation 
steering maneuvers 

Time relative to RMSTS of the end of the translational accelera- 
tion phase of the line-of-sight steering - 

Time relative to RMSTS of the end of the translational coast 
phase of the Ifne-of-sight steering 

Time relative to RMSTS of the end of the 1 ine-of-si'ght steering 
maneuver 

■Unit vector defining the axis of rotation from the initial 
orientation of the end effector to the final orientation of the 
end effector. 

Unit vector along the line-of-sight from the initial end effector 
position to the final end effector position in the orbiter body 
system . 

End effector translational velocity command in the orbiter body 
system 

Magnitude of maximum allowable angular velocity of the end 
effector relative to the orbiter 

Magnitude of maximum allowable translational velocity of the 
end effector relative to the orbiter 

End effector angular velocity command in the orbiter body system 

Magnitude of maximum angular velocity attained for the end 
effector. relative to the orbiter . 

Magnitude of the maximum translational velocity attained for the 
end effector relative to the orbiter. 
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Variable 


Definition 


TRMS Current time relative to RMSTS used for 1 ine-of-sight and axis 

of- rotation steering 

SVDS INPUTS FOR RMS KINMAT OPTION 
I RMS 

=0 RMSB17 and RMSRB6 (ft) required 
^0 RMSANG must be input 

IRMSTR 

=3 Tables TARAT^. i=l ,6 must be input for commanded joint rates 
=1 RMSAMR , (deg/sec 2 ), RMSAMT (ft/sec 2 ), RMSRF (ft), RMSTBF, 
RMSYMR (deg/sec), RMSVMT (ft/sec) required. 

Other required inputs: 

RMSLTH(ft) , RMSRIB(ft) , RMSTLB 

TABLE INPUTS 
The tables 

TARAT- i=l ,6 must use RMSRC as their dependent variable and RMS as 

their independent variable. 
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APPENDIX A 


COORDINATE SYSTEMS AND NOTATIONS 

The purpose of this appendix is to present the definitions of the coordinate 
systems used ih the RMS kinematic model implemented into the SVDS program. 
This appendix is concerned only with right-handed unit orthonormal systems. 
The transformation matrix from any one coordinate system to any other 
coordinate system is a 3 * 3 orthogonal matrix with its determinant equal 
to +1. 


EULER ROTATIONS 

The notation (1^.} i =1 , 2 , 3 is used to denote the components of a unit vector: 

{ l-,} 1 = [ 1 , 0 , 0 ] 

{1 2 } T = [0, 1, 0] (A-l) 

0 3 } T = [ 0 , 0 , 1] 


The notation of a bar under the symbol for a column matrix denotes the 3 x 3 
skew symmetric cross product matrix. 


If- 


then 


(A} T = 


[9-j 3^] 


0 -a 3 a 2 . 


[A] = | a 3 0 -a ] 


l-a 2 a ] 0 


(A-2) 


(A-3) 


Using this notation, it can be shown that the orthogonal matrix corresponding 
to a rotation through an angle 8 about a unit vector u whose components are 
{u> can be written as 

IB] = ( r Ij - sin. 6 [u] + (1 - cos 0)[u] 2 ) (A-4) 
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So that if {V A > is the representation of a vector in the A system and {V g } is 
the representation of the same vector in the B system, and the B system is 
obtained by rotating the A system through an angle 8 about IT, then 

{V B > = [B]{V a > (A— 5) 

where [B] is defined by eq. (A-4). In developing the relationships obtained 
by rotating coordinate systems about various coordinate axes, it becomes 
convenient to define the following transformations which are sometimes called 
Euler rotations. 

CT i (8)3 = ( r Ij - sin 0 0. J 4 (1 - cos 0 ) 3 2 ) (A-6) 

Writing these transformations out explicitly: 

1 0 0 1 

IT ^ ( 0 ) ] = 0 cos 0 sin 0 

_0 -sin 9 cos 6_ 

"cos e 0 -sin 0 

[T 2 (0)] =010 (A-7) 

jsin 0 0 cos 0 

~cos 0 sin- 0 
[T 3 ( 0 ) ] = -sin e cos e 
0 0 

A definition of the coordinate systems used in the RMS kinematics follows. 
COORDINATE SYSTEMS 

The orbiter body coordinate system is a standard airplane system with the 
x-axis or 1-axis defined as positive forward, the y-axis or 2-axis is defined 
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as positive outthe right wing, and the z-axis or 3-axis is positive down, as 
shown in the figure below. 



System 1 is the longeron system, which is fixed in- the longeron supporting the 
arm. The x-axis or 1-axis is positive. aft, the z-axis is downward and per- 
pendicular to the longeron, and the y-axis is to the left, completing the 
right-hand system. This system is rolled outboard through an angle <J> during 
RMS deployment activity. The transformation matrix from the body system to 
the longeron system can be written as 

IT lb ] = EV^J 


-10 0 
0 - 1 , 0 
0 0 1 


(A-8)- 


System 2 is fixed in the upper arm and is obtained from the longeron system by 
first yawing through the shoulder yaw angle and the pitching through the ' 
shoulder pitch angle. The transformation matrix from system 1 to system 2 can 
be written as 
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I T 2 i 1 = [T 2 (0 2 )][T 3 (e 1 )] 


(A-9) 


System 3 is fixed in the lower arm, and is obtained by rotating about the ' 
2-axis through /the elbow pitch angle, 0^. The transformation from system 2 
to system 3 can be written as: 

[T 32 ] = [ V 0 3 }] (A- 10 ) 

System 4 is obtained by rotating system 3 about the z-axis through 0^, the 
wrist pitch angle. The transformation from system 3 to system 4 is written: 

lT 43 j = I T 2 (e 4 )] (A- 11) 

System 5 is obtained by rotating system 4 about the 3-axis through 6 5 , the- 
wrist- yaw angle. The transformation from system 4 to system 5 is written 

- ^ 54 ] - ^3 (^ 5 )] (A-12) 

I 

System 6 is fixed in the end effector and is obtained by rotating system 5 
about the 1-axis. The transformation from system 5 to system 6 is: 

/ t 6 5 1 - IT/e/] (fl-13) 

. \ 

In most cases, we are interested in transforming the component of vectors to 
the longeron or system 1 or to the body system. Using the transformations 
defined above we can write: 

[ V * [T i 2 J‘ T 23i-tW <fl - 14) 

and [T bj ] = tT LB ) T rB 1;j ] (A-15) 

where we have used the fact that 

. [.V - tT d1 J T (*-16) 
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The convention 
transformed to, 


is followed of labeling as the first .index the coordinate system 
and. as the second index the coordinate system transformed from. 
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APPENDIX B 


RMS ANGLE INITIALIZATION OR COMPUTATION 

The purpose of this appendix is to present a derivation of the equations used 
to compute the arm angles given the position of the tip of the end effector 
and the orientation of system 6* relative to the orbiter body system. 

This problem can be reduced to computing the arm angles starting with the 
position vector of the RMS orbiter attach point to the tip of the end effector 
expressed in the longeron system and the transformation matrix from the end 
effector system to the longeron system. Definitions of certain arm lengths 
used in these computations follow. 

£.| The. distance from the orbiter arm attach point to the shoulder pitch 

joint 

The length of the upper arm from the shoulder pitch joint to the' el bow 
pitch joint 

The length of the lower arm from the elbow pitch joint to the. wrist 
pitch joint 

& 4 The distance from the wrist pitch joint to the wrist yaw joint 

j lr The distance from the wrist yaw joint to the tip of the end effector 

Using this information, the transformation from the end effector system to the 
longeron system can be written 

[1*1 6 ] = [T 3 (e 1 )] T [T 2 (9 2 + e 3 + e 4 )] T [T 3 (e 5 )] T [T 1 (9 6 )] T (B-l) 

Also, the component of the position vector from the orbiter attach poir 
the tip of the end effector in the longeron system can be written 


See Appendix A. 


B-l 


{f 16 } " ' {1 3 U 1 + £ T i2 ]{1 l U 2 * IT 13 H1 1 H 3 + IT 14 ]{1 1 H 4 + [T 16 ]{1 1 K 5 


(B-2) 


From eq. (B-2), 


{r^} {r 1g } + n 3 }^ - [T 16 H1-|H 5 




( B— 3 ) 


where {r-j} is the position vector from the^ho^lder pitch joint to the wrist 
yaw joint. It can be seen that the projection of this vector onto the x-y 
longeron plane defines the shoulder yaw angle 


or 


tan 6 

0-j = tan - ^ 


^(2) 

i - 


( B— 4) 


(r , (2)/r, (1)) 


Then 

[B] = [B 16 ] T [T 3 (6 1 )] T = [T 1 (e 6 )][T 3 (e,)][T ? (B)] (B-5) 


where 

Expanding eq. (B-5),- 



1 0 0 

[B] = 

o ce c se c 
6 6 


o -se 6 ce 6 

From eq. 

(B-7), 


6=0, 

+ e 3 + 9 4 

5 S0 5 ° 


fee o -SB - 

5 C0 5 0 


o 

o 

0 1 


SB 0 C3 


e 5 = sin -1 (B ( 1 ,2)) 



(B-6) 


cq 5 cb 

Se 5 

-C0 5 SB 

C6 5 CB 

ce 6 C8 5 

-C0 5 sb 

C0 5 CB 

-S0 6 C9 5 

-C9 5 SB 


(B— 7 ) 

(B-8) 

( B— 9) 


B-2 



(B-10) 


B = tan 


1 -B(1,3J\ 

vb^M/ 


Using eq. (B-3), 


(P> = [13(6-, )]{r 1 > = [T 2 {e 2 )] T {l 1 }& 2 


■ T 


+ [T 2 (e 2 + e 3 )] l {i 1 H 3 + it 2 (b)] {i-,H 4 


From this, we can write 

{■p} = ce 2 {i 1 }£ 2 - se 2 {i 3 H 2 + c{0 2 + e 3 )n 1 }£ 3 


and 


or 


- S0 2 (1 3 }£ 3 + C3{1-|}£^ - S 3 tl 3)^4 


p 1 = C0 2 £ 2 + C(0 2 + 0 3 )£ 3 + CB£ 4 
P 3 = -S0 2 £ 2 - S ( ©2 + 9 3 )£ 3 - S3£ 4 

C0 2 £ 2 + C(e 2 + e 3 )£ 3 = P-J - CB£ 4 = a 

se 2 £ 2 + s(e 2 + e 3 )£ 3 = -p 3 - s $£ 4 = y 


Squaring both equations, we obtain 


C 2 0 2 £ 2 + 2C0 2 C(0 2 + 0 3 )£ 2 £ 3 + C 2 (0 2 + 0 3 )£ 2 
S 2 0 2 £ 2 + 2 S 0 2 S ( 0 2 + 0 3 )£ 2 £ 3 + S 2 ( 0 2 + 0 3 )£ 2 


01 


= Y 


Adding these equations, we obtain 


? 2 2 2 
£ 2 + 2£ £ £ 3 C0 3 + £ 3 = a + y 


or 


e 3 = ±cos 


2 A 2 2 2’ 
_1ja + y - £ 2 - £ 3 

2£ 2 £ 3 


(B- 11 ) 

(B- 12 ) 

(B-13) 

(B-14) 

(B-15X 

(B-16) 

(B-17) 


B-3 



From the geometry of the RMS, we know that 0^ is always negative; 


e 3 = -cos 


-1 


(a + y - r z - C 

2£ ^ 3 _ 


From eq. (B-14)» we can write 


ce 2 £ 2 + ce 2 ce 3 £ 3 - Se 2 se 3 Ji 3 = a 


S0 2 £ 2 + SG 2 C0 3 & 3 + C6 2 S0 3 & 3 - y 


Now 


se 2 (& 2 + £ 3 ce 3 ) = y - ce 2 se 3 s. 3 
y - ,ce 2 se 3 Ji 3 


se 


2 & 2 + £ 3 C© 3 


Substituting eq, (B-21) into the first of eqs. (B-19), we have 


/y - ce ? se q 5,o\ 

c{e 2 )U 2 * + C8 3 & 3 ) - + & 3 C 0 3 j se 


3^3 a 


Solving for © 2 » we h ave 


0 O = COS 


-j/a(£ 2 + ^ ) + ySQ-5^ 


3 3 


*| + & 3 + 2^ 2 £ 3 C6 3 


Using eq. (B-6), we have 


9 4 = B - 0 2 - 0 3 


This completes the derivation. 


therefore , 

( B— 1 8) 

(B-19) 

(B-20) 

(B-21) 

(B-22) 

(B-23) 

(B-24) 


B-4 



APPENDIX C 


THE RESOLVED RATE LAW 

The purpose of -this. appendix is to present a derivation of the Resolved Rate 

I , 

Law, which is a relationship between the translation and angular velocity of 
the tip of the end effector and the joint angle rates. This relationship may 
be denoted in the following manner: 

6x1 

{to}! 6x1 6x1 

= [A] {5} (C-l) 

_{V) 

where 

{V} is a column matrix of the relative translational velocity in the orbiter 
body system 

{to} is a column matrix of the component of the relative angular velocity 
[A] is a 6 x 6 matrix commonly referred to as a Jacobian matrix 
.{0} is a 6 x 1 column matrix of the joint angle rates. 

Let denote a unit base vector in the i direction of system j , then the 
angular velocity of the end effector can be written 

w = 6^ + (0 2 + e 3 + 0 4 ) + e 5 e^ + e g e^ (C-2) 

Before proceeding, note the following useful relationships: 

[T. (0}J{1 1 > = {1^} = [T.(0)] T {1.} (C-3) 


H " 1 " ' ' r ' 1 ~ " 

The same' coordinate systems are used as defined in Appendix A. 


C-l 


and 


[T.(e)]{i.} = ce{i } - se-n } x n } 

I J ' J 

[T i <0)] T {lj} = cedj} + S0{1.} X {!.} (C-4) 

Using the transformation presented in Appendix A and- eq, (C-2), we can write 
[V 0 2 + 6 3 + V] [W]^ = [ t 2 (6 2 + 0 3 + 0 4 )] {1 3 >®i + {1 2 }( ®2 + 0 3 + 

+ (1 3 }0 5 + = c(e 2 + 0 3 + e 4 ){i 3 > - s(e 2 + e 3 + 0 4 ){i 1 } 0 1 

+ (1 2 }(0 2 + § 3 + 0^) + ^3^5 + + S 0 5^2^6 (^- 5 ) 

Eq. (C-5-) gives one of the relationships we want. The second , equation, rela- 
tive translational velocity to joint rates, can be developed by writing the 
position vector from- the shoulder pitch joint to the .tip of the end effector 
and differentiating this relationship to arrive at the desired results. 

The components of the position vector expressed in the longeron system can be 
written 

{ r } = [T 3 (e 1 )] T [T 2 (0 2 )] T {l 1 H 2 + [ T 3 C e ! >] T [ T 2 (0 2 + 9 3 ) ] T{1 1 U 3 
+ fV 9 l^] [V 0 2 + 0 3 + e 4^]^1^4 

+ jT 3 (0- 1 )J T [T 2 ( 0 2 + 0 3 + V] T [ T 3^ e 5^] T{ V £ 5 ( C-6 ) 

Differentiating eq. (C-6), we get 


C-2 




After performing a considerable amount of algebra on eq. (C-7), the 
result can be obtained. 

Jt 2 (e 2 + e 3 + e 4 )][W] m = e 1 ^-c(e 2 + e 3 + o 4 )se 5 a 5 U 1 } - s(e 2 + e 3 + o 4 )so 5 «. 5 0 3 > - 

+ (C0 2 * 2 + c(e 2 + g 3 )£ 3 + C(o 2 + e 3 + e 4 )U 4 + CG 5 £ 5 )jn 2 ^ 

+ o 2 ((s(e 3 + e 4 )n 2 + so 4 £ 3 )n 1 ) + (-C(o 3 + 6 4 H 2 - m 4 « 3 - U 4 + ce 5 i 5 

+ ® 3 5 V3 f V + (- C V3 - { h + Ce 5V)<V 
+ e 4 (- ^ + C0 5 V {1 3 } ) + h -( S0 sW + c Vs { V) 

Eqs. (C-5) and (C-8) are the equations to be solved. First* let us 
following definitions. 



t <v i 5 )) 

(C-7) 

following 

>)<v) • 

(C-8) 
make the 

( C-9 ) 


C-3 



(c-10) 


" b 4l 


T 2 (e 2 + e 3 + e 4 ) t 3 (© 1 ) {v> 


Also, we will define 

a ll = _S ( 0 2 + 0 3 + V * a 16 = C0 5 * a 22 * a 23 = a 24 = a 35 = 1 

a 25 - * a 3] - ^3 * ®/j.) » = ~C(0 2 ®3 

a 51 = ce 2 £ 2 + c(e 2 + e 3 )5, 3 + c(e 2 + e 3 + e 4 )U 4 + ce 5 ^ 5 ) 

a 61 = - S(0 2 + 0 3 + 6 4 )S0 5 £ 5 

a 42 = S(0 3 + 6 4 )£ 2 + S V 3 

a 62 = " C ^ 0 3 + 0 4^2 " C9 4 £ 3 “ ^4 + C0 5%^ 

a 43 = S6 4 £ 3 

a 63 = _C0 4 £ 3 " ^4 + C0 sV 

a 64 = "^4 + C0 5 A 5^ 

a 45 = ’* S0 5 Jl 5 * a 55 = C9 5 5 '5 

Using the definitions in eqs. (C-9) — (C-ll), we can write 

[A] {0} = {b} 


or . 


C-4 


(C-ll) 

(c-12) 



a ll 

. 0 

0 

0 

0 

a l6 




V 

0 

1 

1 

1 

0 

a 26 




b 2 

a 31 

0 

0 

0 

1 

0 




b 3 

a 41 

a 42 

a 43 

0 

a 45 

0 


6 4 


b 4 

a 51 

0 

0 

0 

a 55 

0 


h 


b 5 

a 61 

a 62 

a 63 

a 64 

0 

0 


it 


_ b 6 


Because of the sparseness of the [A] matrix in eq. (C-13), it is 
to write the 0's explicitly. ‘ From eq. ’(C-13) we can write 


a 31 1 


5 i 


b 3 

a 51 a 55 


! 5 . 


>_ 


"®1 

1 

a 55 

-1 


b 3 


" a 31 a 55 ‘ a 51 

" a 51 

a 31 


b 5 
_ _ 


Also from eq. (C-13) 


or 


b l ~ a ll 0 l + a 16°6 




This leaves: 


t i r 


0 2 


b 2 ‘ a 26®6 

a 42 a 43 0 


e 3 

= 

b 4 “ a 41®l “ a 45®5 

a 62 a 63 a 64 


it 


b 6 “ a 61®l 


(C-13) 


convenient 


(Ct14) 


(C-15) 


(C-16) 


(C-17) 


C-B 



which yields 



" 





h 


a 43 a 64 

a 63 ‘ a 64 

7 a 43 


b 2 “ a 26®5 


i 

" a 42 a 64 




b 4 - a 41 -1 ‘ a 45 

a 43 ta 64 " a 62 } ~ a 42 ta 64 " a 63^ 

a 64 ’ a 62 

a 42 


it 

- 

a 42 a 63 " a 62 a 43 

a 62 ' a 63 

a 43 ‘ a 42 


b 6 ‘ a 61®l 


‘ ' (0-1 8) 

This completes the development of the desired equations. It should he noted 
that the following singularity conditions exist, which follow from the divisors 
appearing in eqs, (C-15), (C-16), and (C-18): 

‘ cq 2 £ 2 + c(9 2 + e 3 U 3 + c(e 2 + © 3 + e 4 )& 4 = o • 

. . C6g = 0 , S0 3 = .° • . ' (C-19) 


C-6 



APPENDIX D 


LINE-OF-SIGHT STEERING AND AXIS-OF-ROTATION STEERING 

The purpose of this appendix is to develop the equations used in the line-of- 
sight steering and the axis-of-rotation steering of the SVDS RMS kinematic 
capability. In line-of-sight steering, the tip of the end effector is commanded 
to move along a line from its initial position to its final commanded position. 
In axis-of-rotation steering, the end effector coordinate system is rotated 
about its single axis of rotation to change ,its orientation from its initial 
orientation to its final commanded orientation. 


LINE-OF-SIGHT STEERING 

The direction of the line of sight is first determined, and a unit vector in 
this direction. is computed. 

{Ar} = {R f } - {Rj} ( D— 1 ) 

where • 

{R^} is the final desired position vector expressed in the orbiter system 
relative to the CM of the 'orbiter 

{Rj} is the initial position vector 

{Ar} is the position vector representing the change in position desired 
Compute- the distance to be traveled as 


d = V{Ar} T {Ar> 

and the unit vector in the direction of the desired velocity as 

{u T > = l {Ar} 


( D— 2 ) 


(D-3) 


D-l 



Next, assuming an initial velocity of zero and an acceleration and velocity 
limit, compute the times of the acceleration discontinuities- for a trapezoidal 
velocity time history. Determine if maximum velocity will be reached by 
computing - 


2 

d = r 


m 


m m 


(D-4) 


If d is greater than d , then maximum velocity will be reached and the velocity 
command profile will be 


t 


1 





+ t-, 


“ h + t l 


(D— 5 ) 


and 


{ V 


(V c ) 


( V 


a t{u T } 0 < t < t, 
m T — 1 

W t, < t < t 2 

a m tt; 3 - V‘i' 3 


(D-6) 


If d is greater than d, then maximum velocity limit will not be reached and 
the actual maximum velocity can be computed as 



CD-7) 


This leads to the following times and velocity commands: 


★ 




t 


3 



D-2- 


(D-8) 



{V c } = a m t{u T > 0 < t < tl 

(V c } = a m (t 3 - t){u T } 0 < t < t 3 (D-9) 

AXIS-OF-ROTATION STEERING' 

The algorithm for computing the desired angular velocity command requires the 
transformation matrix to the initial orientation and the’ transformation to 
the desired final orientation. The transformation from the initial orienta- 
tion is then computed and the axis of rotation and the angle of rotation are 
extracted from this matrix. 

[Bpj] = [B p ] [Bj] T (D-10) 


Using eq. (A-4), we can write 



[Bp j 3 = ( r lj - sin 0 [u] + (1 - cos 0 ) [ uj 2 ) 

(D-ll) 

From (D-ll), we 

see that 



IB] - M 2 — = sin 6 [u] 

(D-12) 

Eq. (D-12) can 

be used to compute the axis of rotation if 0 is 

not equal to 

180°. Compute 




W 1 = ^ B 32 " B 23 ^/ 2 



W 2 = ^ B 13 ' B 31 V 2 



W 3 = < B 21 - B !2>/ 2 

(D-13) 

and 

{W} = sin 0 {u} 

(D-14) 

so 

sin 0 = V{W} T {W} 

(D-15) 

and 

< u > = „ «} 

(D-16) 


D-3 



If sin 0 = 0, then it must be determined if the angle of rotation is 0 or ' 
180° and if 0 = 180°, what is the axis of rotation. The trace of [Bpj] is 
examined: if -the trace is 3, then 0 = 0; if the trace is 1, then 0 = 180°. 

: To determine the axis of rotation for the case where 0 = 180°, the following 
formulas can be used 


EC]- - j ([B pI j - r I_,) = \ (l - cos 9)[uj 2 = [uj z 


-'(4 * 4 ) U 1 U 2 u l u 3 

u,u 2 -(u* + u 2 u 3 

u l“3 u 2“3 '( 4 * 4 ) 


(D-17) 

3 

and.' X] u i =1 ' . (D-18) 

i=l 


From the diagonal elements of IC] the magnitudes of the components of {u} 
can be determined using the following formulas. 

. Abs (ui ) = ( C n ‘ C 22." C 33)’ 

Abs(u 2 ) = ( c 2 2 - - C 33 ) 

AbsCu^) = (c 33 - C-, -j - C 22 )‘ (D-19) 


Next, determine the component with maximum magnitude and assume it to be 
positive. Assuming is this element, then 


C.. 

u. = 

1 u o 


o i i 


( 0 - 20 ) 


The same technique is used for the determination of the magnitude of the 
commanded angular velocity, as was used in the line-of~sight steering. • 
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